/********************************************
 * Field & Service Robotics Course 2011
 * Team Mars (Group 5)
 * Manipulator Class File
 * Dorian Tsai, Tilman Pfieffer, Roberto Sosa
 * 2011 November 6
 *
 * The manipulator class houses all of the functions required to tele-operate and control the
 * Mars Manipulator with the J2B2 robot.
 ********************************************
 */

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include "manipulator.h"
//#include "serialstd.hpp"
#include "owndebug.h"
#include <assert.h>

using namespace std;

//#define USB_MICRO "/dev/ttyACM0"
#define TORAD 0.017453292
#define TODEG 57.29577951
#define PIhalf 1.570796326794897

//initialization
std::string USB_MICRO;
float S;

/* CONSTRUCTOR
 * manipulator class constructor
 * link lengths and joint angles are set. Positions are then automatically calculated using forward kinematics
 */
Manipulator::Manipulator(float l1, float l2, float s, float e, float w, float v, float g) {
    // physical parameters
    L1 = l1; // length of link 1 from shoulder to elbow
    L2 = l2; // length of link 2 from elbow to linear actuator

    // joint/actuator positions
    // note that s,e,w are received in [rad] from the client computer
    // v is received in [mm] from the client computer
    S = s; // angle of shoulder joint [rad] in manipulator frame
    E = e; // angle of elbow joint [rad]
    W = w; // angle of wrist joint [rad] - currently not implemented - set to zero!
    V = v; // vertical position of the linear actuator [mm]

    // end-effector pose
    // TEST IF THIS ASSIGNMENT ACTUALLY CHANGES THE ATTRIBUTES OF THE MAN. OBJECT!!
    X = L1*cos(S) + L2*cos(S+E); // x position [mm] left when in J2B2 position looking down and behind (double check - might have changed)
    Y = L1*sin(S) + L2*sin(S+E); // y position [mm] right
    Z = V; // z position [mm] vertical
    C = S+E+W; // rad

    // gripper position
    G = g; // gripper position [deg] (since it is not used in mathematical calculations!)

    USB_MICRO="/dev/ttyACM0"; //usb port of microcontroller
}

/* SET USB
 * sets the usb device (name) used on the computer, since Atmega microcontroller seems to change board names easily
 */
    void Manipulator::setUSB(std::string usb) {
  USB_MICRO=usb;
}

/* SET JOINTS SEV W + G
 * sets the joint / actuator angles and positions of the manipulator class and gripper
 * once limits are confirmed, the manipulator state (internal model) is updated and
 * commands are sent to the manipulator microcontroller
 */
void Manipulator::setJoints(float shoulder, float elbow, float vertical, float wrist, float gripper) {
    // calculate would-be pose of manipulator using fwdKinematics
    float *pos=fwdKinematics(shoulder,elbow,vertical,wrist);

    //check limits of would-be pose
    // if limits are ok, update the state of the manipulator
    if (checkLimits(pos[0],pos[1], pos[2], pos[3], shoulder, elbow, vertical, wrist, gripper)) {
        // updating state of manipulator
        S = shoulder;
        E = elbow;
        V = vertical;
        W = wrist;

        G = gripper;

        // X,Y,Z and C are also set to ensure internal model matches real world
        X = pos[0];
        Y = pos[1];
        Z = pos[2];
        C = pos[3];

        // having updated the state, corresponding commands are sent to the man. uC
        writeMicro();
    }
    else{dPrint(1,"Commands exceed limits! Command NOT sent!");}
}

/* SET POSE XYZ C + G
 * sets the manipulator pose (position and orientation) and gripper
 */
void Manipulator::setPose(float xPos, float yPos, float zPos, float c) {

    //invKinematics(xPos,  yPos,  zPos , c);
    float *angles=invKinematics(xPos,  yPos,  zPos , c);

    // check limits of would-be position
    if(checkLimits(xPos, yPos, zPos, c, angles[0], angles[1], angles[2], angles[3], G)){
        // recall that capital X etc are manipulator state variables
        X = xPos;
        Y = yPos;
        Z = zPos;
        C = c;
        

        S = angles[0];
        E = angles[1];
        V = angles[2];
        W = angles[3];

        // write positions to microcontroller
        writeMicro();
    }
}

/* SET new positions and angles
 * receives command from server and executes either position or angle changes
 * depending on what was put in
 */
void Manipulator::Set(float x,float y,float z, float c,  float s, float e, float v, float w, float g){
	if (x != X || y != Y || z != Z || c != C){
	  setPose(x,y,z,c);
	}
	if (s != S || e != E || v != V || w != W || g != G ){
	  set Joints(s,e,v,w,g);
	}
}

/*  GET JOINTS SEVW + G
 * returns the joint positions and vertical position as a ptr to an array
 */
float *Manipulator::getJoints() {
    float *ang = new float[5];
    ang[0] = S;
    ang[1] = E;
    ang[2] = V;
    ang[3] = W;
    ang[4] = G;
    return ang;
}

/* GET POSE XYZC + G
 * returns the position of the end effector and its orientation as a ptr to an array
 */
float *Manipulator::getPose() {
    float *pose = new float[4];
    pose[0] = X;
    pose[1] = Y;
    pose[2] = Z;
    pose[3] = C;
    return pose;
}

// simple get functions for individual access
float Manipulator::getX() {
    return X;
}

float Manipulator::getY() {
    return Y;
}
float Manipulator::getZ() {
    return Z;
}
float Manipulator::getC() {
    return C;
}
float Manipulator::getS() {
    return S;
}
float Manipulator::getE() {
    return E;
}
float Manipulator::getW() {
    return W;
}
float Manipulator::getV(){
    return V;
}
float Manipulator::getG(){
    return G;
}

/* CHECK LIMITS
 * check limits of the manipulator class to ensure everything is within the physical limits of operation
 * everything is within limits => returns okay = one, else okay = 0
 * TO DO: ACCOUNT FOR PHYSICAL LIMITS OF MANIPULATOR AND SHELF WALLS!
 */
int Manipulator::checkLimits(float x, float y, float z, float c, float s, float e, float v,float w, float g) {

    int okay = 1; // set default for okay status (okay = 1 is "within limits")

    float rMax = L1 + L2; // fully extended
    //float rMin = L1*L1 + L2*L2 - 2*L1*L2*cos(180.0*TORAD - e);
    float rMin = L1 - L2; // fully bent, which is no longer possible
    float zMax = 256.0; // fully extended downwards to the ground[mm]
    float zMin = 0.0; // fully retracted
    float planarPos = sqrt(x*x + y*y);
    // check for range limits
        if (planarPos > rMax) {
            dPrint(1,"CHECK ERROR: planarPos > rMax" );
            okay = 0;
        }
        if (planarPos < rMin) {
            dPrint(1,"CHECK ERROR: planarPos < rMin");
            okay = 0;
        }
        if (z > zMax) {
            dPrint(1,"CHECK ERROR: z > zMax");
            okay = 0;
        }
        if (z < zMin) {
            dPrint(1,"CHECK ERROR: z < zMin");
            okay = 0;
        }
        if (g<80 || g>140) {
            dPrint(1,"CHECK ERROR: g exceeded");
            okay = 0;
        }

    // check for servo limits
    if ( (s < -PIhalf)||(s>PIhalf) || (e<-PIhalf) || (e>PIhalf) ) {
        dPrint(1, "CHECK ERROR: s=%f or e=%f exceeded",s*TODEG,e*TODEG );
        okay = 0;
    }
    return okay;
}

/* FWD KINEMATICS
 * given joint positions, calculates the xyz,c positions (ie, pose)
 * based on basic SCARA robot configuration
 * coordinate system defined in final report (in progress)
 */
float *Manipulator::fwdKinematics (float s, float e, float v,float w) {
    float *tipPose = new float[4];

    // do calc for x,y,z and theta
    float x = L1*cos(s) + L2*cos(s+e); // TEST IF THIS ASSIGNMENT ACTUALLY CHANGES THE ATTRIBUTES OF THE MAN. OBJECT!!
    float y = L1*sin(s) + L2*sin(s+e);
    float z = v;
    float c = s+e+w; // rad

    tipPose[0] = x;
    tipPose[1] = y;
    tipPose[2] = z;
    tipPose[3] = c;
    return tipPose;
}

/* INVERSE KINEMATICS
 * given the xyz,c pose, this function calculates the right-handed version of the joint positions
 */
float *Manipulator::invKinematics (float x, float y, float z, float c) {
    float *angles= new float[4];

    // right-handed equations from SCARA Robots Kinematics Example (available under Literature in FSR Dropbox)
    float x2y2 = x*x + y*y;
    float e = acos((x2y2 - L1*L1 - L2*L2)/(2*L1*L2));
    cout << "DEBUG: " << e *TODEG<< endl;
    float sPlusQ = atan2(y,x);
    float q = acos((x2y2 + L1*L1 - L2*L2)/(2*L1*sqrt(x2y2)));
    float s = sPlusQ - q;
    float w = c - s - e;
    float v = z; // - z0

    angles[0] = s;
    angles[1] = e;
    angles[2] = v;
    angles[3] = w;
    return angles;
}

/* SEND JOINT INSTRUCTIONS TO MICROCONTROLLER
 * writes joint positions to microcontroller via serial communication from MaCI library
 * uses specific format: $As###e###v###g###;
 * $ specifies the start of a new message
 * A specifies actuator control
 * --
 * s specifies shoulder servo
 * ### specifies the desired position in degrees
 * e specifies elbow servo
 * --
 * v specifies the linear actuator
 * ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
 * ex. v127; would tell the actuator to go to the halfway point
 * --
 * g specifies the gripper servo
 * ### specifies the desired position in degrees
 * ; specifies the end of the message
 * ex. g090; would tell the servo to go to 90 degrees
 */
void Manipulator::writeMicro() {
	// create serial obj
	CSerialStd ardu;
	printf("Try to write to microcontroller\n");

	// open serial port
	assert( ardu.SetPort(USB_MICRO,9600) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

    //convert angles from floats to integers to decimal values of those integers
    //shoulder
	float sTemp = S*TODEG;
	int convert = (int) sTemp + 90; // add 90 to avoid minus signs. uC subs 90.
    int sends[3];
    sends[0]= (convert - (convert %100))/100 +48; // add 48 for '0' ASCII character number offset
	sends[1]= (convert %100 - convert%10)/10 +48;
	sends[2]= convert %10 +48;
    //shoulder
	float eTemp = E*TODEG;
	convert = (int) eTemp + 90;
    int sende[3];
    sende[0]= (convert - (convert %100))/100 +48;
	sende[1]= (convert %100 - convert%10)/10 +48;
	sende[2]= convert %10 +48;
    // vertical
	int vertical= (int) V;
	int sendv[3];
	sendv[0]= (vertical - (vertical %100))/100 +48;
	sendv[1]= (vertical %100 - vertical%10)/10 +48;
	sendv[2]= vertical %10 +48;
    //gripper
	convert = (int) G;
    int sendg[3];
    sendg[0]= (convert - (convert %100))/100 +48;
	sendg[1]= (convert %100 - convert%10)/10 +48;
	sendg[2]= convert %10 +48;

    // combine all number arrays into one large char array
	char data[19];
	data[0] = (char) '$';
	data[1] = (char) 'A';
	data[2] = (char) 's';//startbyte shoulder
	data[3] = (char) sends[0];
	data[4] = (char) sends[1];
	data[5] = (char) sends[2];
	data[6] = (char) 'e';//startbyte elbow
	data[7] = (char) sende[0];
	data[8] = (char) sende[1];
	data[9] = (char) sende[2];
	data[10] = (char) 'v';//start byte lA
	data[11] = (char) sendv[0];
	data[12] = (char) sendv[1];
	data[13] = (char) sendv[2];
	data[14] = (char) 'g';	// Start byte gripper
	data[15] = (char) sendg[0];// angle gripper
	data[16] = (char) sendg[1];
	data[17] = (char) sendg[2];
	data[18] = (char) ';';	// end byte
	assert( ardu.Write((unsigned char*)data,19) == 19);

	// handshake?
	assert( ardu.Close() == CASRoboBus::KBusStatusOK);
	printf("Test sent and closed.\n");

}

// read from microcontroller via serial communication from MaCI library
/*char Manipulator::readMicro() {

	// create serial obj
	CSerialStd ardu;
	printf("Try to read from microcontroller\n");

	// open serial port
	assert( ardu.SetPort("/dev/ttyUSB0",9600) == CASRoboBus::KBusStatusOK);
	assert( ardu.Open() == CASRoboBus::KBusStatusOK);

	// read from serial port
	// Note: serialtest.cpp uses ReadBlock, which does not appear in the header file...
	unsigned char buf[15]; // buffer to read data to
	buf[14] = '\0';
	assert( ardu.ReadBlock(buf,14,5000) == 14); // not sure what the 5000 is for

	// print out what was read
	fprintf(stderr, "Read: ");
	for (EACH_IN_i(array)) {
		fprintf(stderr, "%02x", buf[i]);
	}
	fprintf(stderr,"\n");

	return buf;
}*/
/*
char Manipulator::convertangle(float in) {

	int degree= (int) in*TODEG;
	char convert = char(degree);
	return convert;
}*/

